/*
 * @Author: huangyupei huangyupei2021@ia.ac.cn
 * @Date: 2024-11-03 20:59:49
 * @LastEditors: huangyupei huangyupei2021@ia.ac.cn
 * @LastEditTime: 2025-03-18 22:24:34
 * @FilePath: /SelfCalib_OptiAcoustic/src/hand_eye_calibration.cc
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "hand_eye_calibration.h"

using namespace std;
using namespace Eigen;

// 使用Eigen库求解手眼标定
// cam_list: 相机的位姿变化
// sonar_list: 声呐的位姿变化
// X: 手眼标定结果，即相机坐标系到机器人坐标系的转换矩阵
void handEyeCalibration(const std::vector<Eigen::Matrix4d> &cam_list,
                        const std::vector<Eigen::Matrix4d> &sonar_list,
                        Eigen::Matrix3d &R, Eigen::Vector3d &pm) {
    size_t n = cam_list.size();
    
    // ------------------旋转部分求解------------------------
    // 求取外参旋转矩阵R的方程为：A = RB
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, n);
    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, n);

    
    // 构建旋转轴(矩阵形式)，由旋转矩阵到旋转向量
    for (int i = 0; i < n; i++) {
        Eigen::Matrix3d RA = sonar_list[i].block<3, 3>(0, 0); // 取出A中的旋转部分
        Eigen::AngleAxisd rotA_vector(RA);

        Eigen::Matrix3d RB = cam_list[i].block<3, 3>(0, 0); // 取出B中的旋转部分
        Eigen::AngleAxisd rotB_vector(RB);

        A.block<3, 1>(0, i) = rotA_vector.axis();
        B.block<3, 1>(0, i) = rotB_vector.axis();
    }

    
    // 计算矩阵 M = AB^T 还是 M = BA^T ? 两种都可以
    // 按照GPT的原理和过程，应该令 M = B A^T, 然后对M奇异值分解后，直接令 X = VU^T
    // 按照正确的Kabsch算法，应该令 M = A B^T, 然后对M奇异值分解后，直接令 X = UV^T
    // Matrix3d M = B * A.transpose();
    Matrix3d M = A * B.transpose();

    // 对 M 进行奇异值分解 (SVD)
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, ComputeFullU | ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // 计算旋转矩阵 R = U * V^T
    R = U * V.transpose();

    // 检查并确保 det(X) = 1，以满足旋转矩阵的要求
    if (R.determinant() < 0) {
        // 如果 det(X) = -1，则调整 U 的最后一列符号
        U.col(2) *= -1;
        R = U * V.transpose();
    }
    // ----------------- 旋转部分求解完成 ------------------

    // X.block<3, 3>(0, 0) = R;
    // cout << "R_sc: \n" << R << endl;

    
    // ------------------平移部分求解------------------------    
    // 传统手眼标定方案：只求解3维的平移向量
    // 求取外参平移向量的方程是：Mx = b, M的维度为 n*3, x的维度为 3*1, b的维度是 n*1
    Eigen::MatrixXd A_p = Eigen::MatrixXd::Zero(3 * n, 3);
    Eigen::MatrixXd b = Eigen::MatrixXd::Zero(3 * n, 1);
    
    for (size_t i = 0; i < n; ++i) {
        Eigen::Matrix3d RA = sonar_list[i].block<3, 3>(0, 0);
        Eigen::Vector3d pA = sonar_list[i].block<3, 1>(0, 3); // 取出A中的平移部分
        Eigen::Vector3d pB = cam_list[i].block<3, 1>(0, 3); // 取出B中的平移部分
        // cout << "norm of t_cam: " << pB.norm() << endl;
        A_p.block<3, 3>(3 * i, 0) = Eigen::Matrix3d::Identity() - RA;
        b.block<3, 1>(3 * i, 0) = pA - R * pB;
    }

    // 利用正规方程求解平移向量pm
    pm = (A_p.transpose() * A_p).ldlt().solve(A_p.transpose() * b);

    // // 计算伪逆并求解pm
    // MatrixXd A_pseudo = pseudoinverse(A_p);
    // auto pm = A_pseudo * b;
    
    // ----------------- 平移部分求解完成 ------------------
    
    // X.block<3, 1>(0, 3) = pm;
    // cout << "p_sc: \n" << pm.transpose() << endl;
}


void handEyeCalibration(const std::vector<Eigen::Matrix4d> &cam_list,
                        const std::vector<Eigen::Matrix4d> &sonar_list,
                        Eigen::Matrix3d &R, Eigen::Vector3d &pm, double &scale) {
    size_t n = cam_list.size();
    
    // ------------------旋转部分求解------------------------
    // 求取外参旋转矩阵R的方程为：A = RB
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, n);
    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3, n);

    
    // 构建旋转轴(矩阵形式)，由旋转矩阵到旋转向量
    for (int i = 0; i < n; i++) {
        Eigen::Matrix3d RA = sonar_list[i].block<3, 3>(0, 0); // 取出A中的旋转部分
        Eigen::AngleAxisd rotA_vector(RA);

        Eigen::Matrix3d RB = cam_list[i].block<3, 3>(0, 0); // 取出B中的旋转部分
        Eigen::AngleAxisd rotB_vector(RB);

        A.block<3, 1>(0, i) = rotA_vector.axis();
        B.block<3, 1>(0, i) = rotB_vector.axis();
    }

    
    // 计算矩阵 M = AB^T 还是 M = BA^T ? 两种都可以
    // 按照GPT的原理和过程，应该令 M = B A^T, 然后对M奇异值分解后，直接令 X = VU^T
    // 按照正确的Kabsch算法，应该令 M = A B^T, 然后对M奇异值分解后，直接令 X = UV^T
    // Matrix3d M = B * A.transpose();
    Matrix3d M = A * B.transpose();

    // 对 M 进行奇异值分解 (SVD)
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, ComputeFullU | ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // 计算旋转矩阵 R = U * V^T
    R = U * V.transpose();

    // 检查并确保 det(X) = 1，以满足旋转矩阵的要求
    if (R.determinant() < 0) {
        // 如果 det(X) = -1，则调整 U 的最后一列符号
        U.col(2) *= -1;
        R = U * V.transpose();
    }
    // ----------------- 旋转部分求解完成 ------------------

    // X.block<3, 3>(0, 0) = R;
    // cout << "R_sc: \n" << R << endl;

    
    // ------------------平移部分求解------------------------
    // 最新方案: 将尺度当做一维, 与平移向量构成一个四维的未知向量, 通过最小二乘求解
    // 求取混合向量的方程是 Hx = b, H的维度是3n*4, x的维度是4*1,b的维度是3n*1, n是相机、声呐位姿序列的数量
    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3 * n, 4);
    Eigen::MatrixXd b = Eigen::MatrixXd::Zero(3 * n, 1);
    
    for (size_t i = 0; i < n; ++i) {
        Eigen::Matrix3d RA = sonar_list[i].block<3, 3>(0, 0);
        Eigen::Vector3d pA = sonar_list[i].block<3, 1>(0, 3); // 取出A中的平移部分
        Eigen::Vector3d pB = cam_list[i].block<3, 1>(0, 3); // 取出B中的平移部分
        H.block<3, 3>(3 * i, 0) = Eigen::Matrix3d::Identity() - RA;
        H.block<3, 1>(3 * i, 3) = R * pB;
        b.block<3, 1>(3 * i, 0) = pA;
    }

    // 利用正规方程求解平移向量p_sc和尺度scale
    Eigen::Vector4d x;
    x = (H.transpose() * H).ldlt().solve(H.transpose() * b);
    pm = x.head<3>();
    scale = x[3];
    
    // // 传统手眼标定方案：只求解3维的平移向量
    // // 求取外参平移向量的方程是：Mx = b, M的维度为 n*3, x的维度为 3*1, b的维度是 n*1
    // Eigen::MatrixXd A_p = Eigen::MatrixXd::Zero(3 * n, 3);
    // Eigen::MatrixXd b = Eigen::MatrixXd::Zero(3 * n, 1);
    
    // for (size_t i = 0; i < n; ++i) {
    //     Eigen::Matrix3d RA = sonar_list[i].block<3, 3>(0, 0);
    //     Eigen::Vector3d pA = sonar_list[i].block<3, 1>(0, 3); // 取出A中的平移部分
    //     Eigen::Vector3d pB = cam_list[i].block<3, 1>(0, 3); // 取出B中的平移部分
    //     // cout << "norm of t_cam: " << pB.norm() << endl;
    //     A_p.block<3, 3>(3 * i, 0) = Eigen::Matrix3d::Identity() - RA;
    //     b.block<3, 1>(3 * i, 0) = pA - R * pB;
    // }

    // // 利用正规方程求解平移向量pm
    // pm = (A_p.transpose() * A_p).ldlt().solve(A_p.transpose() * b);

    // // // 计算伪逆并求解pm
    // // MatrixXd A_pseudo = pseudoinverse(A_p);
    // // auto pm = A_pseudo * b;
    
    // ----------------- 平移部分求解完成 ------------------
    
    // X.block<3, 1>(0, 3) = pm;
    // cout << "p_sc: \n" << pm.transpose() << endl;
}

// ~ 测试旋转矩阵到旋转向量的转换
// Eigen::Matrix3d R_test;
// // R_test = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(0.6, 0.8, 0)).toRotationMatrix();
// R_test << C_list[1].block<3, 3>(0, 0);
// cout << "R_test:\n" << R_test << endl;

// auto [axis, angle] = getRotationAxis(R_test);
// std::cout << "Rotation Axis: " << axis.transpose() << std::endl;
// std::cout << "Rotation Angle:" << angle << std::endl;

// Eigen::AngleAxisd rot_vector(R_test);
// cout << "axis_direct: " << rot_vector.axis().transpose() << endl;
// cout << "angle_direct: " << rot_vector.angle() << endl;

// Eigen::Matrix3d R_recov = Eigen::AngleAxisd(rot_vector.angle(), rot_vector.axis()).toRotationMatrix();
// cout << "R_recov:\n" << R_recov << endl;

// 计算矩阵的伪逆
MatrixXd pseudoinverse(const MatrixXd& A, double tol) {
    JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
    double tolerance = tol * max(A.cols(), A.rows()) * svd.singularValues().array().abs().maxCoeff();
    VectorXd singularValuesInv = svd.singularValues().array().abs().inverse();
    singularValuesInv = (svd.singularValues().array() > tolerance).select(singularValuesInv, 0);
    MatrixXd A_pseudo = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
    return A_pseudo;
}


void calcRelativeMotion(vector<Sophus::SE3d> vCamPoses, vector<Sophus::SE3d> vSonarPoses, vector<Eigen::Matrix4d> &cam_list, vector<Eigen::Matrix4d> &sonar_list) {
    for (int i = 1; i < vCamPoses.size(); i++) {
        // 计算相机每两帧之间的相对运动
        auto T_cam_lc = vCamPoses[i - 1].matrix().inverse() * vCamPoses[i].matrix();      // T_cam{i-1}_cam{i}
        // cout << "T_c" << i << "_c" << i + 1 << ": \n" << T_cam_lc.matrix() << endl;
        cam_list.emplace_back(T_cam_lc.matrix());

        // 计算声呐每两帧之间的相对运动
        auto T_sonar_lc = vSonarPoses[i - 1].matrix().inverse() * vSonarPoses[i].matrix();
        // cout << "T_s" << i << "_s" << i + 1 << ": \n" << T_sonar_lc.matrix() << endl;
        sonar_list.emplace_back(T_sonar_lc.matrix());
    }
}

/// @brief estimateScale 函数返回最小二乘估计的比例系数，使得 𝑘⋅𝑣1 与 𝑣2 的误差最小。
/// @param v1 
/// @param v2 
/// @return 
double estimateScale(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) {
    double dot_product_v1_v2 = v1.dot(v2);
    double dot_product_v1_v1 = v1.dot(v1);

    // 检查v1是否为零向量
    if (dot_product_v1_v1 == 0) {
        throw std::invalid_argument("v1 is a zero vector, scale cannot be determined.");
    }

    // 计算比例系数k
    double k = dot_product_v1_v2 / dot_product_v1_v1;
    return k;
}

double estimateScale(const std::vector<Eigen::Vector3d>& vA, const std::vector<Eigen::Vector3d>& vB) {
    assert(vA.size() == vB.size() && "Input vectors must have the same size.");
    size_t N = vA.size();
    double numerator = 0.0; // 用于存储分子
    double denominator = 0.0; // 用于存储分母

    for (size_t i = 0; i < N; ++i) {
        numerator += vA[i].dot(vB[i]);      // a_i^T * b_i
        denominator += vA[i].squaredNorm(); // |a_i|^2
    }

    return numerator / denominator;
}